#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     B_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     B_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     F_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     F_Left,        tmotorTetrix, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("debugStream")
#pragma DebuggerWindows("joystickGame")
//#pragma DebuggerWindows("joystickSimple")

#include "JoystickDriver.c"

task RPM(){
	while(true){
	ClearTimer(T2);
	nMotorEncoder[F_Left] = 0;
	nMotorEncoder[F_Right] = 0;
	nMotorEncoder[B_Left] = 0;
	nMotorEncoder[B_Right] = 0;
	wait10Msec(10);

	float RPM_F_L, RPM_F_R, RPM_B_L, RPM_B_R;
	int time_2 = time10[T2];
  int time_2_min = (time_2 / 100);

	//writeDebugStreamLine("ME:  FL: %d, FR: %d, BL: %d, BR: %d", nMotorEncoder[F_Left], nMotorEncoder[F_Right], nMotorEncoder[B_Left], nMotorEncoder[B_Right]);

	RPM_F_L = (abs(nMotorEncoder[F_Left]) / 6) / time_2_min;
	RPM_F_R = (abs(nMotorEncoder[F_Right]) / 6) / time_2_min;
	RPM_B_L = (abs(nMotorEncoder[B_Left]) / 6) / time_2_min;
	RPM_B_R = (abs(nMotorEncoder[B_Right]) / 6) / time_2_min;

	//writeDebugStreamLine("RPM_F_L: % 7f, RPM_F_R: % 7f, RPM_B_L: % 7f, RPM_B_R: % 7f", RPM_F_L, RPM_F_R, RPM_B_L, RPM_B_R);


  }
}


   int anLookup[] = {
	// We've noticed that the joystick values can be up to 128!
	-100, -50, -25, -12, -6, -3, -1, 0, 0, 1, 3, 6, 12, 25, 50, 100, 100};


int motorSmoother(int before, int after){
	int nSmooth = 7;
	if (before < after){
			after = before + nSmooth;
		}
	if(before > after){
		after = before - nSmooth;
	}
	else if(before == after)
	{
		return after;
	}

	return after;
 }


int convertJoystick2Motor(int y){
	if ((y < -128) || (y > 128)) {
		writeDebugStreamLine("Problem with input!  y=%d", y);
	}
	return (anLookup[(y + 128) / 16]);
}




task teleop(){



	while(true) {
	  int current_FR = motor[F_Right];
	  int current_FL = motor[F_Left];
	  int current_BR = motor[B_Right];
	  int current_BL = motor[B_Left];


		getJoystickSettings(joystick);
		int X1_1C = joystick.joy1_x1;
		int X2_1C = joystick.joy1_x2;
		int Y1_1C = joystick.joy1_y1;

		if(abs(X1_1C) < 20)
		{
			X1_1C = 0;
		}
		if(abs(X2_1C) < 20)
		{
			X2_1C = 0;
		}
		if(abs(Y1_1C) < 20)
		{
			Y1_1C = 0;

		}

		if(abs(X2_1C) > 20 || abs(Y1_1C) > 20)
		{
			X2_1C = 0;
		}


		int X1_1 = convertJoystick2Motor(X1_1C);
		int X2_1 = convertJoystick2Motor(X2_1C);
    int Y1_1 = convertJoystick2Motor(Y1_1C);



	  int F_R = -(-Y1_1 + X1_1 + X2_1);
	  int F_L = -( Y1_1 + X1_1 + X2_1);
	  int B_R = -(-Y1_1 - X1_1 + X2_1);
	  int B_L = -( Y1_1 - X1_1 + X2_1);

		else if(X1_1C < 20)
		{
	   motor[F_Right] = motorSmoother(current_FR, F_R);
	   motor[B_Right] = motorSmoother(current_BR, B_R);
	   motor[F_Left]  = motorSmoother(current_FL, F_L);
	   motor[B_Left] = motorSmoother(current_BL, B_L);
	  }

	  /*if(joy1Btn(1) == 1)
	  {
	  	PlayImmediateTone(2200, 1);
	  }*/


	  writeDebugStreamLine("FL: % 4d, FR: % 4d, BL: % 4d, BR: % 4d", motor[F_Left], motor[F_Right], motor[B_Left], motor[B_Right]);

	  //	writeDebugStreamLine("J1:<% 4d(% 4d), % 4d(% 4d)>, J2:<% 4d(% 4d)>, ME:FL<% 9d> FR<%9d>, BL<% 9d>, BR<% 9d>", joystick.joy1_x1, X1_1, joystick.joy1_y1, Y1_1, joystick.joy1_x2, X2_1,
		                  //   nMotorEncoder[F_Left], nMotorEncoder[F_Right], nMotorEncoder[B_Left], nMotorEncoder[B_Right]);


	  wait10Msec(1);

  }
}


task main()
{

	StartTask(teleop);
	//StartTask(RPM);
	while(true){
		wait1Msec(50);
	}


}
